#include <ESP8266WiFi.h>
#include <WiFiUdp.h>
#include <Servo.h>

const char* ssid = "JIXIEBI";
const char* password = "123456789";
const char* apSSID = "DATA";
const char* apPassword = "1234567890";

const int servoPin1 = D5; //控制右臂
const int servoPin2 = D6; //控制左臂
const int servoPin3 = D7; //控制爪子
const int servoPin4 = D8; //控制底座

WiFiUDP udp;
Servo servo1;
Servo servo2;
Servo servo3;
Servo servo4;

void setup() {
  Serial.begin(115200);

  WiFi.mode(WIFI_AP_STA);
  WiFi.begin(ssid, password);

  int max_attempts = 20;
  int attempts = 0;
  while (WiFi.status() != WL_CONNECTED && attempts < max_attempts) {
    delay(1000);
    Serial.print("正在连接到WiFi...");
    Serial.print(" 尝试次数: ");
    Serial.println(attempts + 1);
    attempts++;
  }

  if (WiFi.status() == WL_CONNECTED) {
    Serial.println("已连接到WiFi网络");
    Serial.print("STA IP地址: ");
    Serial.println(WiFi.localIP());
  } else {
    Serial.println("连接WiFi失败");
    return;
  }

  if (WiFi.softAP(apSSID, apPassword)) {
    Serial.println("AP模式已启动");
    Serial.print("AP IP地址: ");
    Serial.println(WiFi.softAPIP());
  } else {
    Serial.println("AP模式启动失败");
    return;
  }

  servo1.attach(servoPin1);
  servo2.attach(servoPin2);
  servo3.attach(servoPin3);
  servo4.attach(servoPin4);

  servo1.write(0);  // 初始化位置
  servo2.write(120);  // 初始化位置
  servo3.write(0);  // 初始化位置
  servo4.write(180);  // 初始化位置

  udp.begin(1234);
  Serial.println("UDP接收初始化完成");
}

void loop() {
  int16_t ax, ay, az;
  if (udp.parsePacket()) {
    udp.read((uint8_t*)&ax, sizeof(ax));
    udp.read((uint8_t*)&ay, sizeof(ay));
    udp.read((uint8_t*)&az, sizeof(az));

    controlServo(ax, ay, az);
    delay(100);
  }
}

void controlServo(int16_t ax, int16_t ay, int16_t az) {
  int servoAngle1 = map(ay, -16384, 16383, 0, 180);
  int servoAngle2 = map(ay, -16384, 16383, 0, 180);
  int servoAngle3 = map(az, -16384, 16383, 0, 180);
  int servoAngle4 = map(ax, -16384, 16383, 0, 180);

  servoAngle1 = constrain(servoAngle1, 0, 180);
  servoAngle2 = constrain(servoAngle2, 0, 180);
  servoAngle3 = constrain(servoAngle3, 0, 180);
  servoAngle4 = constrain(servoAngle4, 0, 180);

  servo1.write(servoAngle1);
  servo2.write(servoAngle2);
  servo3.write(servoAngle3);
  servo4.write(servoAngle4);

  Serial.print("接收端接收数据:ax: "); Serial.println(ax);
  Serial.print(" ay: "); Serial.println(ay);
  Serial.print(" az: "); Serial.println(az);
  Serial.print(" Servo1 Angle: "); Serial.println(servoAngle1);
  Serial.print(" Servo2 Angle: "); Serial.println(servoAngle2);
  Serial.print(" Servo3 Angle: "); Serial.println(servoAngle3);
  Serial.print(" Servo4 Angle: "); Serial.println(servoAngle4);
}
